clear all;
close all;

m11 = m H + m L − m L s2 φL s2 θL m H (m H + m L ) ;
m12 = −m L cθL sφL sθL m H (m H + m L ) ;
m13 = m L cφL c2 θL sφL m H (m H + m L ) ;
m14 = −cφL lmH cθL ;
m15 = sφL sθL lmH ;
m21 = −m L cθL sφL sθL m H (m H + m L ) ;
m22 = m H + m L − m L s2 θL m H (m H + m L ) ;
m23 = m L cφL cθL sθL m H (m H + m L ) ;
m25 = −cθL lmH ;
m31 = m L cφL c2 θL sφL m H (m H + m L ) ;
m32 = m L cφL cθL sθL m H (m H + m L ) ;
m33 = mH + mL ( s2 φL + s2 θL − s2 φL s2 θL ) m H (m H + m L ) ;
m34 = −sφL lmH cθL ;
m35 = −cφL sθL lmH ;
m41 = −cφL lmH cθL ;
m43 = −sφL lmH cθL ;
m44 = m H + m L L2m H mLc2 θL ;
m51 = sφL sθL lmH ;
m52 = −cθL lmH ;
m53 = −cφL sθL lmH ;
m55 = m H + m L l2m H m L ;
m24 = m42 = m45 = m54 = 0;

Mq = [m11 m12 m13 m14 m15;m21 m22 m23 m24 m25;m31 m32 m33 m34 m35;m41 m42 m43 m44 m45;m51 m52 m53 m54 m55];
M11 = [m11 m12 m13;m21 m22 m23;m31 m32 m33];
M12 = [m14 m15;m24 m25;m34 m35];
M21 = [m41 m42 m43;m51 m52 m53];
M22 = [m44 m45;m54 m55];
M1 = [M11 M12];
M2 = [M21 M22];


Qk=[0.1;0;0.4;0.5];
Zk=[0;0;-0.3;-0.4];

T=0.001;
for k=1:1:30000

    time(k)=k*T;
    
%     if time(k)>=5
%         dt1 = k*T*sin(1/(k*T));
%         dt2 = 1/(k*T+1) + 0.5;
%     else
%         dt1 = 0;
%         dt2 = 0;
%     end
%     dt(:,k)=[dt1;dt2];
    
    x1=Qk(1);x2=Qk(2);x3=Qk(3);x4=Qk(4);
    d1=Zk(3);d2=Zk(4);
    zt(:,k)=[x1;x2;x3;x4];

    S = a1*sigma1 + a2*sigma2;
    
    Ueq1 =-M11^(-1)*(M12*tao-M1*V*de + R1*dep );
    Ueq2 =-M21*(M22*tao-M2*V*de + R2*deq );
    Usw =-(a1*M11 + a2*M21)^(-1) * (a1*M11*Ueq2 + (a1*M12 + a2*M22)*d2 + a2*M2*Ueq1 + ι1*S + (h + ι2)*S/(S + )-d1;
    ut(:,k)=[u1;u2];

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,ut(:,jjk),dt(:,k)),tSpan,Qk);
    Qk = Qq(length(Qq),:);
	[tt1,Zq] = ode45(@(t,x) obv(t,x,zt(:,k),ut(:,k)),tSpan,Zk);
    Zk = Zq(length(Zq),:);
     
end

figure(1);
plot(time,zt(1,:),'k',time,zt(2,:),'r',time,zt(3,:),'b',time,zt(4,:),'g');
xlabel('time(s)');ylabel('th');
legend('x1','x2','x3','x4');
grid on;

figure(2);
plot(time,ut(1,:),'k',time,ut(2,:),'r');
xlabel('time(s)');ylabel('U');
legend('U1','U2');
 

